#pragma once

#include <cstdint>
#include <vector>

// MSP通讯协议常量
namespace MSP {
// MSP帧标志位
constexpr uint8_t MSP_V1_FLAG = '$';
constexpr uint8_t MSP_V2_FLAG = '$';

// MSP方向
constexpr uint8_t MSP_V1_DIRECTION_TO_FC = 'M';
constexpr uint8_t MSP_V1_DIRECTION_FROM_FC = 'M';
constexpr uint8_t MSP_V2_DIRECTION_TO_FC = 'X';
constexpr uint8_t MSP_V2_DIRECTION_FROM_FC = 'X';

// MSP标识符
constexpr uint8_t MSP_V1_IDENT = '<';     // 发送给飞控
constexpr uint8_t MSP_V1_RESPONSE = '>';  // 飞控响应

// MSP命令 (基于Betaflight)
enum Command {
  // 通用命令
  MSP_API_VERSION = 1,
  MSP_FC_VARIANT = 2,
  MSP_FC_VERSION = 3,
  MSP_BOARD_INFO = 4,
  MSP_BUILD_INFO = 5,

  // IMU相关命令
  MSP_RAW_IMU = 102,   // out message: 9 DOF raw IMU值
  MSP_ATTITUDE = 108,  // out message: 2 angles 1 heading (姿态角)

  // 设置相关
  MSP_SET_RAW_RC = 200,
  MSP_SET_MODE_RANGE = 212
};

// MSP帧状态
enum FrameState {
  IDLE,
  HEADER_START,
  HEADER_M,
  HEADER_ARROW,
  HEADER_SIZE,
  HEADER_CMD,
  PAYLOAD,
  CHECKSUM,
  COMMAND_RECEIVED
};

// MSP帧结构
struct Frame {
  uint8_t direction;
  uint8_t size;
  uint8_t cmd;
  std::vector<uint8_t> payload;
  uint8_t checksum;

  Frame() : direction(0), size(0), cmd(0), checksum(0) {}

  // 计算校验和
  void calculateChecksum() {
    checksum = size ^ cmd;
    for (uint8_t b : payload) {
      checksum ^= b;
    }
  }

  // 序列化为字节流
  std::vector<uint8_t> serialize() const {
    std::vector<uint8_t> result;
    result.reserve(size + 6);  // $M<[size][cmd][payload][checksum]

    result.push_back(MSP_V1_FLAG);
    result.push_back(MSP_V1_DIRECTION_TO_FC);
    result.push_back(MSP_V1_IDENT);
    result.push_back(size);
    result.push_back(cmd);

    result.insert(result.end(), payload.begin(), payload.end());

    // 计算校验和
    uint8_t crc = size ^ cmd;
    for (size_t i = 0; i < payload.size(); i++) {
      crc ^= payload[i];
    }

    result.push_back(crc);
    return result;
  }
};

// 姿态数据结构 (对应MSP_ATTITUDE)
struct AttitudeData {
  float roll;   // 横滚角 (度)
  float pitch;  // 俯仰角 (度)
  float yaw;    // 偏航角 (度)

  AttitudeData() : roll(0.0f), pitch(0.0f), yaw(0.0f) {}
};

// IMU原始数据结构 (对应MSP_RAW_IMU)
struct RawImuData {
  int16_t acc[3];   // 加速度计原始值 (X, Y, Z)
  int16_t gyro[3];  // 陀螺仪原始值 (X, Y, Z)
  int16_t mag[3];   // 磁力计原始值 (X, Y, Z)

  RawImuData() {
    for (int i = 0; i < 3; i++) {
      acc[i] = 0;
      gyro[i] = 0;
      mag[i] = 0;
    }
  }
};
};  // namespace MSP